
#include "mag_encoder.h"
#include "utils.h"
#include "math.h"
#include "main.h"
#include "senser_cfg.h"


static volatile uint16_t sen_rx_val[2] = {0};
static uint16_t sen_tx_val[2] = {0x00};
static uint8_t SEN_CNT = 1;





SPI_HandleTypeDef senser_spi;
DMA_HandleTypeDef hdma_senser_spi_rx;
DMA_HandleTypeDef hdma_senser_spi_tx;


static void mag_encoder_interface_init(void)
{
    GPIO_InitTypeDef GPIO_InitStruct = {0};

    /* GPIO Ports Clock Enable */
    __HAL_RCC_GPIOC_CLK_ENABLE();
    __HAL_RCC_GPIOB_CLK_ENABLE();
    __HAL_RCC_GPIOA_CLK_ENABLE();
    /* SPIX clock enable */
    SENSER_SPI_CLK_ENABLE();

    /*SPIX MISO*/
    GPIO_InitStruct.Pin = GPIO_MISO_A_Pin;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_MEDIUM;
    GPIO_InitStruct.Alternate = SPIX_MOSI_A_ALTERNATE;
    HAL_GPIO_Init(GPIO_MISO_A_GPIO_Port, &GPIO_InitStruct);
    /*SPIX MOSI*/
    GPIO_InitStruct.Pin = GPIO_MOSI_B_Pin;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_MEDIUM;
    GPIO_InitStruct.Alternate = SPIX_MOSI_B_ALTERNATE;
    HAL_GPIO_Init(GPIO_MOSI_B_GPIO_Port, &GPIO_InitStruct);
    /*SPIX SCLK*/
    GPIO_InitStruct.Pin = GPIO_SCLK_Z_Pin;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_MEDIUM;
    GPIO_InitStruct.Alternate = SPIX_SCLK_Z_ALTERNATE;
    HAL_GPIO_Init(GPIO_SCLK_Z_GPIO_Port, &GPIO_InitStruct);
    /*SPIX CS*/
    GPIO_InitStruct.Pin = SENSER_SPI_CS_Pin;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_MEDIUM;
    HAL_GPIO_Init(SENSER_SPI_CS_GPIO_Port, &GPIO_InitStruct);

    /* SPIX parameter configuration*/
    senser_spi.Instance = SENSER_SPI_INSTANCE;
    senser_spi.Init.Mode = SPI_MODE_MASTER;
    senser_spi.Init.Direction = SPI_DIRECTION_2LINES;
    senser_spi.Init.DataSize = SPI_DATASIZE_16BIT;
    senser_spi.Init.CLKPolarity = SPI_POLARITY_LOW;
    senser_spi.Init.CLKPhase = SPI_PHASE_2EDGE;
    senser_spi.Init.NSS = SPI_NSS_SOFT;
    senser_spi.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_16;
    senser_spi.Init.FirstBit = SPI_FIRSTBIT_MSB;
    senser_spi.Init.TIMode = SPI_TIMODE_DISABLE;
    senser_spi.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
    senser_spi.Init.CRCPolynomial = 7;
    senser_spi.Init.CRCLength = SPI_CRC_LENGTH_DATASIZE;
    senser_spi.Init.NSSPMode = SPI_NSS_PULSE_DISABLE;
    if(HAL_SPI_Init(&senser_spi) != HAL_OK)
    {
        Error_Handler();
    }

    /* SPIX DMA Init */
    /* SPIX_RX Init */
    hdma_senser_spi_rx.Instance = SENSER_SPI_RX_DMA_INSTANCE;
    hdma_senser_spi_rx.Init.Request = SENSER_SPI_RX_DMA_REQUEST;
    hdma_senser_spi_rx.Init.Direction = DMA_PERIPH_TO_MEMORY;
    hdma_senser_spi_rx.Init.PeriphInc = DMA_PINC_DISABLE;
    hdma_senser_spi_rx.Init.MemInc = DMA_MINC_ENABLE;
    hdma_senser_spi_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;
    hdma_senser_spi_rx.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD;
    hdma_senser_spi_rx.Init.Mode = DMA_CIRCULAR;
    hdma_senser_spi_rx.Init.Priority = DMA_PRIORITY_HIGH;
    if(HAL_DMA_Init(&hdma_senser_spi_rx) != HAL_OK)
    {
        Error_Handler();
    }
    __HAL_LINKDMA(&senser_spi,hdmarx,hdma_senser_spi_rx);

    /* SPIX_TX Init */
    hdma_senser_spi_tx.Instance = SENSER_SPI_TX_DMA_INSTANCE;
    hdma_senser_spi_tx.Init.Request = SENSER_SPI_TX_DMA_REQUEST;
    hdma_senser_spi_tx.Init.Direction = DMA_MEMORY_TO_PERIPH;
    hdma_senser_spi_tx.Init.PeriphInc = DMA_PINC_DISABLE;
    hdma_senser_spi_tx.Init.MemInc = DMA_MINC_ENABLE;
    hdma_senser_spi_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;
    hdma_senser_spi_tx.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD;
    hdma_senser_spi_tx.Init.Mode = DMA_NORMAL;
    hdma_senser_spi_tx.Init.Priority = DMA_PRIORITY_HIGH;
    if(HAL_DMA_Init(&hdma_senser_spi_tx) != HAL_OK)
    {
        Error_Handler();
    }
    __HAL_LINKDMA(&senser_spi,hdmatx,hdma_senser_spi_tx);

    /* DMA1_Channel2_IRQn interrupt configuration */
    HAL_NVIC_SetPriority(DMA1_Channel2_IRQn, 1, 0);
    HAL_NVIC_EnableIRQ(DMA1_Channel2_IRQn);
    /* DMA1_Channel3_IRQn interrupt configuration */
    HAL_NVIC_SetPriority(DMA1_Channel3_IRQn, 1, 0);
    HAL_NVIC_EnableIRQ(DMA1_Channel3_IRQn);
}


void mag_encoder_read_start(void)
{
    HAL_GPIO_WritePin(SENSER_SPI_CS_GPIO_Port, SENSER_SPI_CS_Pin, GPIO_PIN_RESET);
    __HAL_DMA_DISABLE(senser_spi.hdmatx);
    senser_spi.hdmatx->Instance->CNDTR = SEN_CNT;
//    hspi1.hdmatx->Instance->CPAR = (uint32_t)&hspi1.Instance->DR;
//    hspi1.hdmatx->Instance->CMAR = (uint32_t)sen_tx_val;
//    __HAL_DMA_ENABLE_IT(hspi1.hdmatx, DMA_IT_TC);
    __HAL_DMA_ENABLE(senser_spi.hdmatx);
}




void mag_encoder_update(void)
{
    HAL_GPIO_WritePin(SENSER_SPI_CS_GPIO_Port, SENSER_SPI_CS_Pin, GPIO_PIN_SET);

    if(sys.cfg.senser_type == SENSER_TLE5012B_TYPE)
    {
        sys.raw_angle = ((sen_rx_val[1] & 0x7FFF) << 1);
        
        if(sys.cfg.motor_dir == 0)
            sys.foc_controller.angle = sys.raw_angle * 0.0054931640625f;//360.0f / 65536.0f
        else
            sys.foc_controller.angle = 360.0f - sys.raw_angle * 0.0054931640625f;//360.0f / 65536.0f
    }
    else if(sys.cfg.senser_type == SENSER_AS5047P_TYPE)
    {
        sys.raw_angle = ((sen_rx_val[0] & 0x3FFF) << 0);
        
        if(sys.cfg.motor_dir == 0)
            sys.foc_controller.angle = sys.raw_angle * 0.021973997436f;//360.0f / 16383.0f
        else
            sys.foc_controller.angle = 360.0f - sys.raw_angle * 0.021973997436f;//360.0f / 16383.0f
    }
    
    /*sys.foc_controller.angle * _2PI / 360.0f * MOTOR_PP*/
    sys.foc_controller.ele_rad = sys.foc_controller.angle * sys.cfg.rad_gain - sys.cfg.rad_offset;

    while(sys.foc_controller.ele_rad > _2PI)
    {
        sys.foc_controller.ele_rad -= _2PI;
    }
    while(sys.foc_controller.ele_rad < 0.0f)
    {
        sys.foc_controller.ele_rad += _2PI;
    }
//    HAL_GPIO_TogglePin(LED_STA_GPIO_Port, LED_STA_Pin);
}


void mag_encoder_init(void)
{
    mag_encoder_interface_init();

    if(sys.cfg.senser_type == SENSER_TLE5012B_TYPE)
    {
        sen_tx_val[0] = 0x8021;
        sen_tx_val[1] = 0xFFFF;
        SEN_CNT = 2;
    }
    else if(sys.cfg.senser_type == SENSER_AS5047P_TYPE)
    {
        sen_tx_val[0] = 0x8000 | AS5047_COMMAND_READ | ANGLECOM_AS5047P_VOL_REG_ADD;
        SEN_CNT = 1;
    }
    
    SET_BIT(senser_spi.Instance->CR2, SPI_CR2_RXDMAEN);
    SET_BIT(senser_spi.Instance->CR2, SPI_CR2_TXDMAEN);
    
    __HAL_DMA_DISABLE(senser_spi.hdmarx);
    senser_spi.hdmarx->Instance->CNDTR = SEN_CNT;
    //dst
    senser_spi.hdmarx->Instance->CPAR = (uint32_t)&senser_spi.Instance->DR;
    //src
    senser_spi.hdmarx->Instance->CMAR = (uint32_t)sen_rx_val;
    __HAL_DMA_ENABLE(senser_spi.hdmarx);
    __HAL_DMA_ENABLE_IT(senser_spi.hdmarx, DMA_IT_TC);
    
    __HAL_DMA_DISABLE(senser_spi.hdmatx);
    senser_spi.hdmatx->Instance->CNDTR = SEN_CNT;
    senser_spi.hdmatx->Instance->CPAR = (uint32_t)&senser_spi.Instance->DR;
    senser_spi.hdmatx->Instance->CMAR = (uint32_t)sen_tx_val;
//    __HAL_DMA_ENABLE_IT(senser_spi.hdmatx, DMA_IT_TC);
    __HAL_SPI_ENABLE(&senser_spi);
}

#if 0
/*根据实际使用的DMA通道修改*/
void DMA1_Channel2_IRQHandler(void)
{
  /* USER CODE BEGIN DMA1_Channel2_IRQn 0 */
//    HAL_GPIO_TogglePin(LED_STA_GPIO_Port, LED_STA_Pin);
  HAL_GPIO_WritePin(SENSER_SPI_CS_GPIO_Port, SENSER_SPI_CS_Pin, GPIO_PIN_SET);
  mag_encoder_update();
  /* USER CODE END DMA1_Channel2_IRQn 0 */
  HAL_DMA_IRQHandler(&hdma_senser_spi_rx);
  /* USER CODE BEGIN DMA1_Channel2_IRQn 1 */
  
  /* USER CODE END DMA1_Channel2_IRQn 1 */
}
#endif

